

namespace
{
#define DEG_TO_RAD(x) ((x * arma::math::pi()) / 180)

void init(std::vector<vector3>& verts, uchar sides, scalar size, vector3 pos)
{
    if(sides)
    {
        verts.resize(sides);
        matrix rot = genRotation(DEG_TO_RAD(360 / sides));
        vector3 v("0 " + std::to_string(-size) + " 0");
        if (!(sides % 2)) v = genRotation(DEG_TO_RAD(180 / sides)) * v;
        for (auto i = verts.begin(); i != verts.cend(); ++i, v = rot * v)
            *i = (v + pos);
    }
}

} // namespace

//template<uchar s>
RegularShape::RegularShape(uchar sides, scalar size, scalar mass, scalar elasticity) :
        RigidBody(mass, elasticity), _sides(sides), _size(size), _vertices() //, _vertices(init(s, _size, _position))
{
    init(_vertices, sides, size, _position);
}

//template<uchar s>
RegularShape::RegularShape(uchar sides, const vector3 &pos, const vector3 &vel, scalar size, scalar mass, scalar elasticity) :
        RigidBody(pos, vel, mass, elasticity), _sides(sides), _size(size), _vertices() //, _vertices(init(s, _size, pos))
{
    init(_vertices, sides, size, _position);
}

//template<uchar s>
RegularShape::RegularShape(const RegularShape &b) :
        RigidBody(b), _sides(b._sides), _size(b._size), _vertices(b._vertices)
{
//	memcpy(_vertices, b._vertices, sizeof(vector3) * _sides);
}

//template<uchar s>
vector3 RegularShape::doGetSupport(const vector3 &d) const
{
    uchar best = 0;
    scalar best_dot = arma::dot(_vertices[0], d);

    for (uchar i = 1; i < _sides; i++)
    {
        scalar dist = arma::dot(_vertices[i], d);
        if (dist > best_dot)
        {
            best = i;
            best_dot = dist;
        }
    }

    return _vertices[best];
}

//template<uchar s>
void RegularShape::integrate(scalar time)
{
    if (_mass)
    {
        auto step = (_velocity * time);
        _position += step;
        for (uint i = 0; i < _sides; ++i)
            _vertices[i] += step;
    }
}

//template<uchar s>
const std::vector<vector3>& RegularShape::vertices() const
{
    return _vertices;
}

//template<uchar s>
scalar RegularShape::size() const
{
    return _size;
}

//template<uchar sides>
void RegularShape::size(scalar s)
{
    scalar scale = s / _size;
    for (auto &v : _vertices)
        v = ((v - _position) * scale) + _position;
    _size = s;
}

uchar RegularShape::sides() const
{
    return _sides;
}

void RegularShape::sides(uchar s)
{
    _sides = s;
    init(_vertices, _sides, _size, _position);
}

void RegularShape::position(scalar x, scalar y, scalar z)
{
    vector3 diff = _position;
    RigidBody::position(x, y, z);
//    init(_vertices, _sides, _size, _position);

    diff -= _position;
    for (vector3 &v : _vertices)
        v -= diff;
}

//RegularShape::Triangle::Triangle(scalar size, scalar mass, scalar elasticity) : RegularShape(3, size, mass, elasticity){}
//RegularShape::Triangle::Triangle(const vector3 &pos, const vector3 &vel, scalar size, scalar mass, scalar elasticity) : RegularShape(pos, vel, 3, size, mass, elasticity){}
//RegularShape::Triangle::Triangle(const RegularShape::Triangle& t) : RegularShape(t){}//3, t._size, t._mass, t._elastic){}
//
//RegularShape::Square::Square(scalar size, scalar mass, scalar elasticity) : RegularShape(3, size, mass, elasticity){}
//RegularShape::Square::Square(const vector3 &pos, const vector3 &vel, scalar size, scalar mass, scalar elasticity) : RegularShape(pos, vel, 3, size, mass, elasticity){}
//RegularShape::Square::Square(const RegularShape::Square& s) : RegularShape(s){}//3, s._size, s._mass, s._elastic){}
//
//RegularShape::Pentagon::Pentagon(scalar size, scalar mass, scalar elasticity) : RegularShape(3, size, mass, elasticity){}
//RegularShape::Pentagon::Pentagon(const vector3 &pos, const vector3 &vel, scalar size, scalar mass, scalar elasticity) : RegularShape(pos, vel, 3, size, mass, elasticity){}
//RegularShape::Pentagon::Pentagon(const RegularShape::Pentagon& p) : RegularShape(p){}//3, p._size, p._mass, p._elastic){}
